package test.crane;

import ptolemy.actor.*;
import ptolemy.kernel.*;
import ptolemy.kernel.util.*;
import ptolemy.data.*;
import ptolemy.data.type.*;

//////////////////////////////////////////////////////////////////////////
//// SensorCheck
/**

@author Ivan Jeukens
@version v 0.1 07/02/2002
*/
public class SensorCheck extends TypedAtomicActor {

    public SensorCheck(CompositeEntity container, String name) throws
            IllegalActionException, NameDuplicationException {
        super(container, name);

        start = new TypedIOPort(this, "start", true, false);
        start.setTypeEquals(BaseType.GENERAL);
        
        poscar = new TypedIOPort(this, "poscar", true, false);
        poscar.setTypeEquals(BaseType.DOUBLE);
        
        swposcarmin = new TypedIOPort(this, "swposcarmin", true, false);
        swposcarmin.setTypeEquals(BaseType.BOOLEAN);
        
        swposcarmax = new TypedIOPort(this, "swposcarmax", true, false);
        swposcarmax.setTypeEquals(BaseType.BOOLEAN);
        
        shutdown = new TypedIOPort(this, "shutdown", true, false);
        shutdown.setTypeEquals(BaseType.GENERAL);

        vc = new TypedIOPort(this, "vc", false, true);
        vc.setTypeEquals(BaseType.DOUBLE);
        
        brake = new TypedIOPort(this, "brake", false, true);
        brake.setTypeEquals(BaseType.BOOLEAN);

        emstop = new TypedIOPort(this, "emstop", false, true);
        emstop.setTypeEquals(BaseType.GENERAL);

        trigger = new TypedIOPort(this, "trigger", false, true);
        trigger.setTypeEquals(BaseType.GENERAL);

        new Attribute(this, "_nonStrictMarker");
    }

    public TypedIOPort start;
    public TypedIOPort poscar;
    public TypedIOPort swposcarmin;
    public TypedIOPort swposcarmax;
    public TypedIOPort shutdown;

    public TypedIOPort vc;
    public TypedIOPort brake;
    public TypedIOPort emstop;
    public TypedIOPort trigger;

    ///////////////////////////////////////////////////////////////////
    ////                         public methods                    ////

    public void initialize() throws IllegalActionException {
        super.initialize();
        _reset();
    }

    public void fire() throws IllegalActionException {
        if(shutdown.isKnown()) {
            if(shutdown.hasToken(0)) {
                _reset();
            }
        }

        switch(_state) {
            case READY : {
                if(start.isKnown()) {
                    if(start.hasToken(0)) {
                        _started = true;
                        // start.get(0);
                        if(poscar.hasToken(0)) {
                            poscarTk = (DoubleToken) poscar.get(0);
                            double posc = poscarTk.doubleValue();
                            boolean minposc = ((BooleanToken) swposcarmin.get(0)).booleanValue();
                            boolean maxposc = ((BooleanToken) swposcarmax.get(0)).booleanValue();

                            _failedCheck = _staticCheck(posc, minposc, maxposc);
                            if(_failedCheck) {
                                emstop.broadcast(new Token());
                                brake.broadcast(new BooleanToken(true));
                            }
                        }
                    }
                }
            } break;

            case STATICCHECK: {
                if(poscar.isKnown() && swposcarmin.isKnown() && swposcarmax.isKnown()) {
                    if(poscar.hasToken(0) && swposcarmin.hasToken(0) && swposcarmax.hasToken(0)) {
                        poscarTk = (DoubleToken) poscar.get(0);
                        double posc = poscarTk.doubleValue();
                        boolean minposc = ((BooleanToken) swposcarmin.get(0)).booleanValue();
                        boolean maxposc = ((BooleanToken) swposcarmax.get(0)).booleanValue();
    
                        _failedCheck = _staticCheck(posc, minposc, maxposc);
                        if(_failedCheck) {
                            emstop.broadcast(new Token());
                            brake.broadcast(new BooleanToken(true));
                        }
                    }
                }
            } break;
            
            case CHECK1: {
                if(poscar.isKnown() && swposcarmin.isKnown() && swposcarmax.isKnown()) {
                    if(poscar.hasToken(0)) {
                        poscarTk = (DoubleToken) poscar.get(0);
                        double posc = poscarTk.doubleValue();
                        boolean minposc = ((BooleanToken) swposcarmin.get(0)).booleanValue();
                        boolean maxposc = ((BooleanToken) swposcarmax.get(0)).booleanValue();
    
                        if(posc <= -5.1 || minposc) {
                            if(posc > -4.9 || !minposc) {
                                emstop.broadcast(new Token());
                                _failedCheck = true;
    
                                brake.broadcast(new BooleanToken(true));
                                vc.broadcast(new DoubleToken(0.0));
                            }
                            else {
                                brake.broadcast(new BooleanToken(false));
                                vc.broadcast(new DoubleToken(VC_CHECK));
                            }
                            _endedCheck = true;
                        }
                        else {
                            brake.broadcast(new BooleanToken(false));
                            vc.broadcast(new DoubleToken(-VC_CHECK));
                        }
                    }
                }
            } break;

            case CHECK2: {
                if(poscar.isKnown() && swposcarmin.isKnown() && swposcarmax.isKnown()) {
                    if(poscar.hasToken(0)) {
                        poscarTk = (DoubleToken) poscar.get(0);
                        double posc = poscarTk.doubleValue();
                        boolean minposc = ((BooleanToken) swposcarmin.get(0)).booleanValue();
                        boolean maxposc = ((BooleanToken) swposcarmax.get(0)).booleanValue();
    
                        if(posc >= 5.1 || maxposc) {
                            if(posc < 4.9 || !maxposc) {
                                emstop.broadcast(new Token());
                                _failedCheck = true;
    
                                brake.broadcast(new BooleanToken(true));
                                vc.broadcast(new DoubleToken(0.0));
                            }
                            else {
                                brake.broadcast(new BooleanToken(false));
                                vc.broadcast(new DoubleToken(-VC_CHECK));
                            }
                            _endedCheck = true;
                        }
                        else {
                            brake.broadcast(new BooleanToken(false));
                            vc.broadcast(new DoubleToken(VC_CHECK));
                        }
                    }
                }
            } break;

            case CHECK3: {
                if(poscar.isKnown() && swposcarmin.isKnown() && swposcarmax.isKnown()) {
                    if(poscar.hasToken(0)) {
                        poscarTk = (DoubleToken) poscar.get(0);
                        double posc = poscarTk.doubleValue();
                        boolean minposc = ((BooleanToken) swposcarmin.get(0)).booleanValue();
                        boolean maxposc = ((BooleanToken) swposcarmax.get(0)).booleanValue();
    
                        if(posc <= 4.9 || minposc) {
                            if(minposc || maxposc) {
                                emstop.broadcast(new Token());
                                _failedCheck = true;
                                brake.broadcast(new BooleanToken(true));
                            }
                            else {
                                brake.broadcast(new BooleanToken(false));
                                trigger.broadcast(new Token());
                            }
                            vc.broadcast(new DoubleToken(0.0));
                            _endedCheck = true;
                        }
                        else {
                            brake.broadcast(new BooleanToken(false));
                            vc.broadcast(new DoubleToken(-VC_CHECK));
                        }
                    }
                }
            } break;
        }

        if(poscar.isKnown() && swposcarmin.isKnown() && swposcarmax.isKnown()) {
            if(!trigger.isKnown()) {
                trigger.broadcastClear();
            }
            if(!vc.isKnown()) {
                vc.broadcastClear();
            }
            
            if(start.isKnown()) {
                if(!emstop.isKnown()) {
                    emstop.broadcastClear();
                }
                if(!brake.isKnown()) {
                    brake.broadcastClear();
                }
            }
        }
    }

    public boolean postfire() throws IllegalActionException {
        if(shutdown.isKnown()) {
            if(shutdown.hasToken(0)) {
                _reset();
                return true;
            }
        }

        switch(_state) {
            case READY : {
                if(_started) {
                    if(poscarTk == null) {
                        _state = STATICCHECK;
                    }
                    else {
                        if(_failedCheck) {
                            _reset();
                        }
                        else {
                            _state = CHECK1;
                            System.out.println("Mudei para check1\n");
                        }
                    }
                }
            } break;

            case STATICCHECK: {
                if(poscarTk != null) {
                    if(_failedCheck) {
                        _reset();
                    }
                    else {
                        _state = CHECK1;
                        System.out.println("Mudei para check1\n");
                    }
                }
            } break;
            
            case CHECK1: {
                if(_endedCheck) {
                    if(_failedCheck) {
                        _reset();
                    }
                    else {
                        _state = CHECK2;
                        _endedCheck = false;
                        System.out.println("Mudei para check2\n");
                    }
                }
            } break;

            case CHECK2: {
                if(_endedCheck) {
                    if(_failedCheck) {
                        _reset();
                    }
                    else {
                        _state = CHECK3;
                        _endedCheck = false;
                        System.out.println("Mudei para check3\n");
                    }
                }
            } break;
            
            case CHECK3: {
                if(_endedCheck) {
                    _reset();
                    System.out.println("cabou\n");
                }
            } break;
        }

        return true;
    }

    ///////////////////////////////////////////////////////////////////
    ////                        private methods                    ////

    private void _reset() {
        _state = READY;
        poscarTk = null;
        _failedCheck = false;
        _endedCheck = false;
        _started = false;
    }

    private boolean _staticCheck(double pos, boolean min, boolean max) {
        if( (pos < -5.1) || (pos > 5.1) || ( min & max ) ) {
            return true;
        }
        return false;
    }
    
    ///////////////////////////////////////////////////////////////////
    ////                      private variables                    ////

    private int _state;
    private boolean _failedCheck;

    private boolean _endedCheck;
    private boolean _started;
    
    private DoubleToken poscarTk;

    private static final int READY = 1;
    private static final int CHECK1 = 2;
    private static final int CHECK2 = 3;
    private static final int CHECK3 = 4;
    private static final int STATICCHECK = 5;

    private static final double VC_CHECK = 0.002;

}
